/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "fc_sensors.h"
#include "filter.h"
#include "math.h"
#include "kalman_2d.h"
#include "fc_error.h"
#define BARO_AVERAGE_SIZE 9
#define GROUN_SAMPLE_COUNT 100
laggedMovingAverage_t baro_average;
// pt1Filter_t baroLpf;
Kalman_t baroKal;
kalman_2d_t baro_kalman;
bool baro_init = false;
bool ground_init = false;
float baro_buffer[BARO_AVERAGE_SIZE];
float baro_ave[BARO_AVERAGE_SIZE];
// float alt_sum = 0;
uint8_t ground_count = 0;
float groud_pressure = 101325;

extern float F1_wave[12];



/**
 * @brief
 *
 * @param P
 * @return float (cm)
 */
float calculate_altitude(float P, float groundpressure, float T)
{
    // const float P0 = 101325;  // 海平面气压，单位：帕斯卡
    const float L = 0.0065;      // 气温垂直递减率，单位：开尔文/厘米
    const float T0 = 273.15 + T; // 海平面气温，单位：开尔文
    const float g = 9.8;         // 重力加速度，单位：米/秒²
    const float M = 0.0289644;   // 空气平均摩尔质量，单位：千克/摩尔
    const float R = 8.314;       // 普适气体常量，单位：焦耳/（摩尔·开尔文）

    // 应用变形后的公式计算海拔
    return (T0 * 100.f / L) * (1 - pow((P / groundpressure), (R * L) / (g * M)));
}
void sensor_baro_handle(struct rt_sensor_data *baro_sensor, struct rt_sensor_data *temp_sensor)
{
    if (baro_sensor->type == RT_SENSOR_CLASS_BARO)
    {
        if (!baro_init)
        {
            // laggedMovingAverageInit(&baro_average, BARO_AVERAGE_SIZE, baro_ave);
            // pt1FilterInit(&baroLpf, pt1FilterGain(50, 0.01f));
            // quickMedianFilter5Init(&baro_mid_filter, BARO_AVERAGE_SIZE, baro_buffer);
            KalmanFilterInitParam(&baroKal, 1, 50);
            baro_init = true;
        }
        if(FC_ERROR_CHECK(FC_IMU_CALIBRATING))
        {
            ground_count = 0;
        }
        if (ground_count < 10)
        {
            ground_count++;
            if (ground_count == 10)
                groud_pressure = baro_sensor->data.baro * 0.001f;
        }

        baro_t *baro = &fc_sensor_data.baro;
        baro->temperature = temp_sensor->data.temp * 0.1f;

        // quickMedianFilter5f();

        // baro->pressure = laggedMovingAverageUpdate(&baro_average, baro_sensor->data.baro * 0.001f);
        // baro->pressure = qiuckMedianFilter9(&baro_mid_filter, baro_sensor->data.baro * 0.001f);
        baro->pressure = baro_sensor->data.baro * 0.001f;
        // float altitude = 4433000.f * (1.0f - powf((baro->pressure / 101325.0f), 0.190295f)); // cm
        float altitude = calculate_altitude(baro->pressure, groud_pressure, baro->temperature);
        // baro->altitude = laggedMovingAverageUpdate(&baro_average, altitude);
        // baro->altitude = altitude;

        baro->altitude = KalmanFilter(&baroKal, altitude);

    }
}
void acc_baro_fusion()
{
    static uint32_t last_time = 0;
    static bool kalman_init = false;
    static float vel_z_lpf;
    uint32_t current_time = rt_tick_get();
    float dt = (float)(current_time - last_time) * 0.001f;

    float acc_z = fc_sensor_data.acc.accSum.z;
    float baro_z = fc_sensor_data.baro.altitude;
    float vel_z = acc_z * dt * 980.f;
    vel_z_lpf += ((vel_z - vel_z_lpf) * 0.1f);
    if ((kalman_init == 0) && (last_time != 0))
    {
        kalman_init = true;
        kalman_2d_init_vel_pos(&baro_kalman, 0.1, 0.1, 0.05, 2, dt);
        // baro_kalman.A[0][1] = dt;
        // baro_kalman.Q[0][0] = 1;
        // baro_kalman.Q[0][1] = 0;
        // baro_kalman.Q[1][0] = 0;
        // baro_kalman.Q[1][1] = 1;
        // baro_kalman.R[0][0] = 5;
        // baro_kalman.R[0][1] = 0;
        // baro_kalman.R[1][0] = 0;
        // baro_kalman.R[1][1] = 5;
    }
    kalman_2d_update_vel_pos(&baro_kalman, baro_z, vel_z_lpf);
    zdrone_state.pos.z = baro_kalman.x_hat[0][0];
    zdrone_state.vel.z = baro_kalman.x_hat[0][1];
    last_time = current_time;
}
